import rclpy  
from rclpy.node import Node    
from geometry_msgs.msg import Twist  
import numpy as np  
from std_msgs.msg import Int32MultiArray
from std_msgs.msg import Int32
from origincar_msg.msg import Sign  # 根据图片的消息类型
from origincar_msg.msg import Data
import math
import time
from math import atan2, sqrt, sin, cos
from ai_msgs.msg import PerceptionTargets



class InertialNavigationNode(Node):  
    def __init__(self):  
        super().__init__('inertial_navigation_node')  # 初始化节点名称

        self.last_log_time_navagation_0 = self.get_clock().now()
        self.last_log_time_navagation_3 = self.get_clock().now()
        
        # 用于判断是否允许开始导航，1为开始，0为停止
        self.flag = 1

        #用于设置状态，0为简单导航，1为避障导航，2为二维码导航，3为停车点导航   
        self.state = 0

        #目标点切换（任务一任务三启动标志）
        self.task = 0

        #self.init_flag = 0


        # 创建目标位置订阅，主要用于订阅目标位置,分别是x.y和角度
        self.subscription_robotpose = self.create_subscription(
            Data,  
            '/robotpose',
            self.robotpose_callback,
            2
        )
        
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.subscription_sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10) 
        
        #二维码识别
        self.subscription_sign_switch = self.create_subscription(  
            Sign,  
            'sign_switch',  
            self.sign_switch_callback,  
            3)
        
        self.timer = self.create_timer(
            timer_period_sec=1/40,  # 时间间隔（秒）
            callback=self.timer_callback  # 回调函数
        )

        self.subscription_fcos = self.create_subscription( 
            PerceptionTargets, 
            '/hobot_dnn_detection',  
            self.fcos_callback, 
            10
        )  

        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)  
  
        # 初始化状态变量用来储存位置
        self.angular =  0
        self.position = np.array([0.0, 0.0])  
        self.target_position = np.array([0.0, 0.0])  # 目标位置     
        self.half_position = np.array([0.0, 0.0])    # 中间点位置

        
        #锥桶的相对坐标在这里，分别是底边中点x,y,宽度和高度
        self.zhuitong_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        #停车点的相对坐标
        self.park_relativ_position = np.array([0.0, 0.0,0.0,0.0])

        self.jishiqi = 0
        self.t1_time = 0
        self.t3_time = 0
    
        #40cm左右，这个坐标是距离40的相对坐标，使用这个比较稳
        self.zhuitongyuzhi_40x = 250
        self.stop_distance = 0.1
        self.vx = 0.8
        self.vx_3 = 0.8
        

        self.current_waypoint_index = 0
        self.waypoint_tolerance = 0.1  
        self.waypoints = [[0.7, 0.2] , [5.0, 1.9] ]

        self.cone_time = self.get_clock().now()
        
                    
    
    def check_waypoint(self):
        # 检查当前目标点索引是否超出范围
        if self.current_waypoint_index >= len(self.waypoints):
            self.get_logger().info("All waypoints completed!")
            return
        
        # 获取当前目标点
        target = self.waypoints[self.current_waypoint_index]
        
        # 计算距离
        distance = math.sqrt(
            (target[0] - self.position[0])**2 +
            (target[1] - self.position[1])**2
        )
        
        # 到达目标点判断
        if distance < self.waypoint_tolerance:
            self.get_logger().info(f"Reached waypoint {self.current_waypoint_index}")
            self.current_waypoint_index += 1
            self.target_position = self.waypoints[self.current_waypoint_index]


    def robotpose_callback(self, msg):
        # 更新机器人的x轴位置坐标
        self.position[0] = msg.x
        # 更新机器人的y轴位置坐标
        self.position[1] = msg.y
        # 更新机器人的角度
        self.angular = msg.z


    def timer_callback(self):
        if self.task == 1 :
            if(self.jishiqi > 0) :
                self.navigation_0(self.new_x,self.new_y)
                self.jishiqi = self.jishiqi - 1
            else:
                self.navigation_control()
        

            

    def sign_switch_callback(self,msg):
        #二维码识别的回调函数，必须要任务1的时候在回调
        if(self.task == 1):
            self.flag = 0
            self.task = 2
            self.state = 0
            #self.init_flag = 0

            cmd_msg = Twist()  
            cmd_msg.linear.x = 0.0
            cmd_msg.angular.z = 0.0
            self.cmd_pub.publish(cmd_msg)  

            
    def fcos_callback(self, msg):
        if (self.task == 1):
            for num, target in enumerate(msg.targets):
                if target.rois:
                
                    class_name = target.rois[0].type.strip()
                # 获取第一个 ROI
                    roi = target.rois[0].rect

                # 获取框参数
                    x_offset = roi.x_offset
                    y_offset = roi.y_offset
                    height = roi.height
                    width = roi.width
                
                if (self.task == 1) :
                    if class_name == 'obstacle': 
                    #底边中点坐标
                        x = int(x_offset + 0.5 * width)
                        y = y_offset + height
                        self.zhuitong_relativ_position = x,y,width,height
                        if (width > 90 and height > 100) : 
                            self.get_logger().info("cone(x,y,width,height)=({} , {} , {} , {})".format(x,y,width,height))
                            if ( y > self.zhuitongyuzhi_40x):
                                self.state = 1 
                                self.get_logger().info("识别到锥桶")
                                self.navigation_1()
                                return 
            self.state = 0
                        
                           

                     
    def sign4return_callback(self,msg): #sign4return话题的回调函数，在foxglove上显示
    # 创建 Sign 消息实例

        if msg.data == -1:
            self.flag = 1
            self.state = 0
            self.task = 1

            self.t1_time = self.get_clock().now()
            #前往二维码区
            self.target_position[0] = 4.8  # 目标x和y坐标
            self.target_position[1] = 1.7      
              
            
            
            self.get_logger().info(f"任务一")
            #self.navigation_init()
            

  
        else:
            self.flag = 0
            self.task = 2
            self.state = 0
            self.get_logger().info(f"任务二")
            self.get_logger().info(f"C区进行遥测")

        return
    
    def navigation_control(self): #总导航控制逻辑
        # 这里是导航控制逻辑
        if self.flag == 1 :
            if self.state == 0 :
                if(self.task == 1):
                    self.navigation_0(self.target_position[0],self.target_position[1])
            elif self.state == 1 :
                self.navigation_1()              
                
                
        else:
            twist_msg = Twist()
            twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
            twist_msg.angular.z = 0.00  # 使用 角速度复位
            # 发布 Twist 消息
            self.cmd_pub.publish(twist_msg)     


    def navigation_init(self) :
        self.get_logger().info('进入init')
        
        if(self.zhuitong_relativ_position[3] > 175 ) :
            if ( self.zhuitong_relativ_position[0] > 178) :    #左转
                self.get_logger().info('初始左转近一点的地方')
                cmd_msg = Twist()  
                cmd_msg.linear.x = self.vx  # 设置线性速度  
                cmd_msg.angular.z = 10.0  # 设置角速度  
                self.cmd_pub.publish(cmd_msg)  # 发布控制指令

                time.sleep(1.2)
                    

            elif(self.zhuitong_relativ_position[0] <= 178 ):    #直行

                cmd_msg = Twist()  
                cmd_msg.linear.x = self.vx  # 设置线性速度  
                cmd_msg.angular.z = 0.0 # 设置角速度  
                self.cmd_pub.publish(cmd_msg)  # 发布控制指令

                time.sleep(1.5)


        if(self.zhuitong_relativ_position[3] > 120 and self.zhuitong_relativ_position[3] <= 175) :
            if ( self.zhuitong_relativ_position[0] > 200) :    #左转
                self.get_logger().info('初始左转远一点的地方')
                cmd_msg = Twist()  
                cmd_msg.linear.x = self.vx  # 设置线性速度  
                cmd_msg.angular.z = 5.0  # 设置角速度  
                self.cmd_pub.publish(cmd_msg)  # 发布控制指令

                time.sleep(1.2)
                


            elif(self.zhuitong_relativ_position[0] <= 200 ):    #直行

                cmd_msg = Twist()  
                cmd_msg.linear.x = self.vx  # 设置线性速度  
                cmd_msg.angular.z = 0.0  # 设置角速度  
                self.cmd_pub.publish(cmd_msg)  # 发布控制指令

                time.sleep(1.9)
        
        self.init_flag = 1


    def navigation_0(self,target_x,target_y): #state=0 无避障简单惯导
        
        # 计算目标点的角度
        dx = target_x - self.position[0]
        dy = target_y - self.position[1]
        if (dx < 0.05 and dy < 0.05):
            twist_msg = Twist()
            twist_msg.linear.x = 0.00  # 示例：将 x 坐标复位
            twist_msg.angular.z = 0.00  # 使用 角速度复位
            self.cmd_pub.publish(twist_msg) 
        
        target_angle_global = atan2(dy, dx)
        angle_diff = target_angle_global - self.angular
        angle_diff = atan2(sin(angle_diff), cos(angle_diff))
        twist = Twist()
        twist.linear.x = self.vx
        twist.angular.z = 1.5 * angle_diff

        self.cmd_pub.publish(twist)  # 发布控制指令
        #self.get_logger().info('self_angular={:.3f}target_x{:.3f}target_y{:.3f}target_angle={:.3f}'.format(self.angular,target_x,target_y,angle_diff))


    def navigation_1(self): #state=1 避障惯导

        #锥桶在右，向左避障,左转的角度比较小
        if(self.zhuitong_relativ_position[0] >= 300 ):
            if(self.task == 1 ):
                    self.get_logger().info(f"任务1向左避障")
                    self.get_logger().info("self_position(x,y)=({:.2f} , {:.2f})".format(self.position[0],self.position[1]))
                    self.new_x = self.position[0] + 0.3 * math.cos(self.angular) - 0.2 * math.sin(self.angular)
                    self.new_y = self.position[1] + 0.3 * math.sin(self.angular) + 0.2 * math.cos(self.angular)
                    self.navigation_0(self.new_x,self.new_y)
                    self.get_logger().info("self_angular= {:.2f} ".format(self.angular))
                    self.get_logger().info("self_target_position(x,y)=({:.2f} , {:.2f})".format(self.new_x,self.new_y))
                    self.jishiqi = 1
                    self.state = 0


                    
        #锥桶在左，向右避障
        elif(self.zhuitong_relativ_position[0] <300 ):
            if(self.task == 1):
                    self.get_logger().info(f"任务1向右避障")
                    self.get_logger().info("self_position(x,y)=({:.2f} , {:.2f})".format(self.position[0],self.position[1]))
                    self.new_x = self.position[0] + 0.3 * math.cos(self.angular) + 0.20  * math.sin(self.angular)
                    self.new_y = self.position[1] + 0.3 * math.sin(self.angular) - 0.20  * math.cos(self.angular)
                    self.navigation_0(self.new_x,self.new_y)
                    self.get_logger().info("self_angular= {:.2f} ".format(self.angular))
                    self.get_logger().info("self_target_position(x,y)=({:.2f} , {:.2f})".format(self.new_x,self.new_y))
                    self.jishiqi = 1
                    self.state = 0
                    
        else:
            self.navigation_0(self.target_position[0],self.target_position[1])



def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation = InertialNavigationNode()  #
    rclpy.spin(navigation)
    navigation.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数

